#include "joypad.h"
#include "SDL/SDL.h"

bool f_lockctrl;
bool f_lockmulti;

byte joyPort[2]={0, 0};
byte joyPortCtrl[2]={0, 0};
byte joyPortData[2]={0xFF, 0xFF};

byte v_jpadhold2=0;
byte v_jpadpress2=0;
byte v_jpadhold1=0;
byte v_jpadpress1=0;
byte v_jpadhold2c2=0;
byte v_jpadpress2c2=0;
byte v_jpadhold1c2=0;
byte v_jpadpress1c2=0;


void pcJoypads(bool *keys)  //this function simulates data port A(controller 1) and B(controller 2) in the genesis/MD 
{                 //this includes the multiplexing accounted for when using buttons B,C or A,Start
     
     joyPortCtrl[0]=joyPort[0];	      //simulates the control port, which we initially write to for change TH pin
     joyPort[0]=joyPortData[0];       //simulates the data port, which is initialized as 11111111

     if (keys[SDLK_UP]) { joyPort[0]=joyPort[0]-(joyPort[0]&1); }
     else { joyPort[0]=joyPort[0]|1; }
     if (keys[SDLK_DOWN]) { joyPort[0]=joyPort[0]-(joyPort[0]&2); }
     else { joyPort[0]=joyPort[0]|2; }
     if (keys[SDLK_LEFT]) { joyPort[0]=joyPort[0]-(joyPort[0]&4); }
     else { joyPort[0]=joyPort[0]|4; }
     if (keys[SDLK_RIGHT]) { joyPort[0]=joyPort[0]-(joyPort[0]&8); }
     else { joyPort[0]=joyPort[0]|8; }
     
     //TH pin controls A,Start  or   C,B
     if ((joyPortCtrl[0] & 64)==0)
     {
        if (keys[SDLK_RETURN]){ joyPort[0]=joyPort[0]-(joyPort[0]&32); }
        else { joyPort[0]=joyPort[0]|32; }
        if (keys[SDLK_a]){ joyPort[0]=joyPort[0]-(joyPort[0]&16); }
        else { joyPort[0]=joyPort[0]|16; }
     }
     else
     {
         if (keys[SDLK_d]){ joyPort[0]=joyPort[0]-(joyPort[0]&32); }
         else { joyPort[0]=joyPort[0]|32; }
         if (keys[SDLK_s]){ joyPort[0]=joyPort[0]-(joyPort[0]&16); }
        else { joyPort[0]=joyPort[0]|16; }
     }
     
     //CONTROLLER 2
     /*
     if (up) { joyPort[1]=joyPort[1]-(joyPort[1]&1); }
     else { joyPort[1]=joyPort[1]|1; }
     if (down) { joyPort[1]=joyPort[1]-(joyPort[1]&2); }
     else { joyPort[1]=joyPort[1]|2; }
     if (left) { joyPort[1]=joyPort[1]-(joyPort[1]&4); }
     else { joyPort[1]=joyPort[1]|4; }
     if (right) { joyPort[1]=joyPort[1]-(joyPort[1]&8); }
     else { joyPort[1]=joyPort[1]|8; }
     
     //TH pin controls A,Start  or   C,B
     if (joyPort[1]&64==0)
     {
        if (start){ joyPort[1]=joyPort[1]-(joyPort[1]&32); }
        else { joyPort[1]=joyPort[1]|32; }
        if (a){ joyPort[1]=joyPort[1]-(joyPort[1]&16);
        else { joyPort[1]=joyPort[1]|16; }
     }
     else
     {
         if (c){ joyPort[1]=joyPort[1]-(joyPort[1]&32); }
         else { joyPort[1]=joyPort[1]|32; }
         if (b){ joyPort[1]=joyPort[1]-(joyPort[1]&16); }
        else { joyPort[1]=joyPort[1]|16; }
     }
     */
     joyPortData[0]=joyPort[0];      //finally rewrite the data port for next call to this function
     
}

void ReadJoypads(bool *keys)       //this function converts the hardware end for data/controller ports A&B into a more usable format, placed at v_jpadhold1 
{
     byte *joypadPtr=&v_jpadhold1;  //this is where the output data starts
     
     for (int portno=0; portno<=1; portno+=1)
     {
         //The joypad output format:
         /*
         Reading a data register returns the state of all pins:

         Bit 7 - Unused
         Bit 6 - TH pin
         Bit 5 - TR pin
         Bit 4 - TL pin
         Bit 3 - Data 3 pin
         Bit 2 - Data 2 pin
         Bit 1 - Data 1 pin
         Bit 0 - Data 0 pin
         */
         
         /*
         Bit 7 - Start
         Bit 6 - A button
         Bit 5 - C button
         Bit 4 - B button
         Bit 3 - Right
         Bit 2 - Left
         Bit 1 - Down
         Bit 0 - Up
         */

         byte *joypadPort=joyPort+portno;
    
         *joypadPort=0;
         //nop  --gives time to allow the setting of joypadPort
         //nop  --to then return the proper data
         //on the pc end since this hardware doesnt exist we simulate with pcJoypads()...
         pcJoypads(keys);
         
         byte joypad1=((*joypadPort)<<2)&0xC0;      //TR & TL pins (bits 4&5) shifted to bits (6&7) so they dont get overwritten when reading port a second time for C&B
         
         *joypadPort=0x40;
         
         //nop  --gives time to allow the setting of joypadPort
         //nop  --to then return the proper data
         //on the pc end since this hardware doesnt exist we simulate with pcJoypads()...
         pcJoypads(keys);

         byte joypad2=((*joypadPort))&0x3F;	  //now read TR & TL pins a second time to grab C&B values, dont get bit 6&7
         
         byte totalJoypad=~(joypad1|joypad2);	  //bits 6&7 replaced with TR & TL pins from first read, A&Start values, joypad1 now holds all testable controller bits 
						  //(LOGICAL NOT ~ TO REVERSE THE BITS AS HARDWARE DOES THIS IN REVERSE)
         byte changebits=*joypadPtr^totalJoypad;  //exclusive or the current state with the previous state to get which bits have changed
         //joypadPtr[0]=((holdval<<8)&0xFF00)
	 byte holdval=totalJoypad;		  //grab current state (for buttons held)         
	 joypadPtr[(portno*2)]=holdval;           //write the current state (this allows above statement to work next run)
         
	 byte pressval=changebits&totalJoypad;	  //and the bits that have changed with current state to yield for only bits that have changed
         joypadPtr[(portno*2)+1]=pressval;	  //this means buttons tapped and not necessarily HELD
         //joypadPtr[0]=joypadPtr[0]+(holdval&0x00FF);
     }
}
     
